namespace Microsoft.Robotics.Navigation.Perception
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.CompilerServices;
    using System.Text;
    using System.Windows;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Used to generate inclusion zone maps for fast enumeration of pixels inside a circular segment
    /// Assumes typical cartesian coordinate system with x growing from left to right, and y growing from bottom up
    /// </summary>
    public class InclusionZoneMarker
    {
        /*  Sample usage of this class
          
            InclusionZoneMarker map = new InclusionZoneMarker();
          
            List<FastTuple<int, int, int>> inclusionZone = map.GenerateInclusionZoneScanLines(poseOfTheCenter, fieldOfViewInRadians, radius);
          
            // That's it, now inclusionZone list contains entries for each scan line with 3-tuples indicating start/end x and y coordinates of all included pixels
            // To enumerate all pixels, use code like below:          
            foreach (var t in inclusionZone)
            {
                int y = t.Item3;

                for (int x = t.Item1; x < t.Item2; ++x)
                {
                    VisitPixel(x, y); // x and y are origin-adjusted 
                }                
            } 
         */

        /// <summary>
        /// A very large number dividing by which is still greater than zero. Need this to avoid singularity cases when computing tangent
        /// of angles close to 0 or Pi. This is done to include points on the map that are lying almost perfectly on the bounding line
        /// </summary>
        private const double AFairlyLargeNumber = 2 * 1000 * 1000 * 1000;

        /// <summary>
        /// Generates scan lines for inclusion zone given original pose, field of view and radius
        /// Coordinate system is consistent with typical cartesian coordinate system (x grows from left to right, y grows from bottom to top)
        /// </summary>
        /// <param name="pose">View origin pose</param>
        /// <param name="fovAngle">Field of view in radians</param>
        /// <param name="radius">Radius of area bounds</param>
        /// <returns>List of 3-tuples. In each tuple: Item1 is the first column (x coordinate adjusted for origin) from where to begin enumerating pixels
        /// Item2 is one position further from last included column (also adjusted for origin, +1 is done to allow later scanning loop use more conventional less than sign when iterating)
        /// and Item3 is the row (y coordinate adjusted for origin) for which there are valid columns
        /// </returns>
        public List<FastTuple<int, int, int>> GenerateInclusionZoneScanLines(Pose2D pose, double fovAngle, int radius)
        {
            if (radius < 0 || fovAngle <= 0 || fovAngle > Math.PI)
            {
                throw new ArgumentException("Invaid value for field of view or radius");
            }

            List<FastTuple<int, int, int>> scanLines = new List<FastTuple<int, int, int>>();

            if (radius == 0)
            {
                // radius of 0 is valid - we just have to return an empty structure.
                return scanLines;
            }
            
            scanLines.Capacity = radius + 1;

            // center point to this control point is what will determine a line and direction of search for bounding line 1
            // we also pre-compute one over tangent of the angle, as that's what we'll need to compute x coordinate on the map
            double oneOverTanThetaLine1;
            Pose2D line1Pose = this.GetBoundingLineCharacteristics(pose.Heading - fovAngle / 2, radius, out oneOverTanThetaLine1);

            // center point to this control point is what will determine a line and direction of search for bounding line 2
            // we also pre-compute one over tangent of the angle, as that's what we'll need to compute x coordinate on the map
            double oneOverTanThetaLine2;
            Pose2D line2Pose = this.GetBoundingLineCharacteristics(pose.Heading + fovAngle / 2, radius, out oneOverTanThetaLine2);

            int originPoseX = (int)pose.X;
            int originPoseY = (int)pose.Y;

            // This is a modified midpoint (brezenheim) algorithm for generating max/min borders of a circle (as opposed to covering all points of a circle
            // which would not be as efficient, since we'd be analyzing some scan lines twice for different values of x along that circle's perimiter
            // Not using PathPlanningCommon::IEnumerable<Point2D<int>> CirclePoints(int radius) due to perf
            int x = radius;
            int y = 0;
            int radiusError = 1 - x;

            // To avoid a check for y == 0 in the loop (which we need to do, otherwise we'd visit y==0 point twice), we special case that here and 
            // call it once for y==0 (which only happens once) we must do everything  we do in the if (radiusError < 0) condition here, so that we can
            // continue the loop from y==1 properly
            this.AnalyzeSymmetricCirclePoint(x, 0, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);
            ++y;
            radiusError += 2 * y + 1;
            
            // The key in this loop is to only analyze points that have a new y (row)
            while (x >= y)
            {
                if (radiusError < 0)
                {
                    this.AnalyzeSymmetricCirclePoint(x, y, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);
                    this.AnalyzeSymmetricCirclePoint(x, -y, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);

                    ++y;

                    radiusError += 2 * y + 1;
                }
                else
                {
                    this.AnalyzeSymmetricCirclePoint(x, y, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);
                    this.AnalyzeSymmetricCirclePoint(x, -y, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);
                    this.AnalyzeSymmetricCirclePoint(y, x, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);
                    this.AnalyzeSymmetricCirclePoint(y, -x, radius, originPoseX, originPoseY, ref line1Pose, ref oneOverTanThetaLine1, ref line2Pose, ref oneOverTanThetaLine2, scanLines);

                    ++y;
                    --x;

                    radiusError += 2 * (y - x + 1);
                }
            }

            return scanLines;
        }

        /// <summary>
        /// Analyze x pixel on both sides of the circle and see if it some part of this scan line belongs to the inclusion zone
        /// </summary>
        /// <param name="x">x coordinate on the scan line</param>
        /// <param name="y">y coordinate on the scan line</param>
        /// <param name="radius">Bounding circle radius</param>
        /// <param name="originX">Origin x</param>
        /// <param name="originY">Origin y</param>
        /// <param name="line1Pose">Line 1 pose</param>
        /// <param name="oneOverTanThetaLine1">One over tangent theta for line 1</param>
        /// <param name="line2Pose">Line 2 pose</param>
        /// <param name="oneOverTanThetaLine2">One over tangent theta for line 2</param>
        /// <param name="inclusionZoneList">List of 3-tuples. In each tuple: Item1 is the row (adjusted for origin) for which there are valid columns, 
        /// Item2 is the first column (adjusted for origin) from where to begin enumerating pixels
        /// and Item3 is one position further from last included column (done to keep scanning loop more conventional)</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private void AnalyzeSymmetricCirclePoint(
            int x,
            int y,
            int radius,
            int originX,
            int originY,
            ref Pose2D line1Pose,
            ref double oneOverTanThetaLine1,
            ref Pose2D line2Pose,
            ref double oneOverTanThetaLine2,
            List<FastTuple<int, int, int>> inclusionZoneList) 
        {
            int circleXStart = -x;
            int circleXEnd = x;

            double zoneStart = circleXStart;
            double zoneEnd = circleXEnd;

            if (line1Pose.Heading >= 0 && this.OrientPoint(line1Pose.X, line1Pose.Y, circleXStart, y) >= 0)
            {
                // Top two quadrants and circle start point is to the left of first line
                // We could take this line out of this condition (since its duplicated later), but that means running this math unnecessarily in some cases
                double xCoordinateForThisYOnLine1 = y * oneOverTanThetaLine1;

                // We are doing this check to make sure we handle the singularity point case where one over tangent is infinity and 
                // multiplying infinity by 0 is undefined. Such condition simply indicates that X is infinity if line heading is exactly 0, and X is 0 otherwise 
                if (0 == y && Math.Abs(oneOverTanThetaLine1) > AFairlyLargeNumber)
                {
                    xCoordinateForThisYOnLine1 = (line1Pose.Heading == 0 ? radius : 0);
                }

                zoneEnd = Math.Min(zoneEnd, xCoordinateForThisYOnLine1);
            }
            else if (line1Pose.Heading <= 0 && this.OrientPoint(line1Pose.X, line1Pose.Y, circleXEnd, y) >= 0)
            {
                // Bottom two quadrants and circle end point is to the left of first line
                // We could take this line out of this condition (since its duplicated above), but that means running this math unnecessarily in some cases
                double xCoordinateForThisYOnLine1 = y * oneOverTanThetaLine1;

                // We are doing this check to make sure we handle the singularity point case where one over tangent is infinity and 
                // multiplying infinity by 0 is undefined. Such condition simply indicates that line X limit should be set to 0 if heading is 0, and to -infinity otherwise 
                if (0 == y && Math.Abs(oneOverTanThetaLine1) > AFairlyLargeNumber)
                {
                    xCoordinateForThisYOnLine1 = (line1Pose.Heading == 0 ? 0 : -radius);
                }

                zoneStart = Math.Max(zoneStart, xCoordinateForThisYOnLine1);
            }
            else
            {
                return; // nothing on this scan line
            }

            if (line2Pose.Heading >= 0 && this.OrientPoint(line2Pose.X, line2Pose.Y, circleXEnd, y) <= 0)
            {
                // Top two quadrants and circle end point is to the right of second line
                // We could take this line out of this condition (since its duplicated later), but that means running this math unnecessarily in some cases
                double xCoordinateForThisYOnLine2 = y * oneOverTanThetaLine2;

                // We are doing this check to make sure we handle the singularity point case where one over tangent is infinity and 
                // multiplying infinity by 0 is undefined. Such condition simply indicates that X should be considered 0 if line heading is exactly 0, and -infinity otherwise
                if (0 == y && Math.Abs(oneOverTanThetaLine2) > AFairlyLargeNumber)
                {
                    xCoordinateForThisYOnLine2 = (line2Pose.Heading == 0 ? 0 : -radius);
                }

                zoneStart = Math.Max(zoneStart, xCoordinateForThisYOnLine2);
            }
            else if (line2Pose.Heading <= 0 && this.OrientPoint(line2Pose.X, line2Pose.Y, circleXStart, y) <= 0)
            {
                // Bottom two quadrants and circle start point is to the right of second line
                // We could take this line out of this condition (since its duplicated above), but that means running this math unnecessarily in some cases
                double xCoordinateForThisYOnLine2 = y * oneOverTanThetaLine2;

                // We are doing this check to make sure we handle the singularity point case where one over tangent is infinity and 
                // multiplying infinity by 0 is undefined. Such condition simply indicates that X should be considered infinity if line heading is exactly 0, and 0 otherwise
                if (0 == y && Math.Abs(oneOverTanThetaLine2) > AFairlyLargeNumber)
                {
                    xCoordinateForThisYOnLine2 = (line2Pose.Heading == 0 ? radius : 0);
                }

                zoneEnd = Math.Min(zoneEnd, xCoordinateForThisYOnLine2);
            }
            else
            {
                return; // nothing on this scan line
            }

            // Time to inject a new entry into the final array. Do it only for when it makes sense (start column is less than end column)
            if (zoneStart <= zoneEnd)
            {
                inclusionZoneList.Add(new FastTuple<int, int, int>((int)(zoneStart + originX), (int)(zoneEnd + originX + 1), originY + y));
            }
        }

        /// <summary>
        /// Gets some point on a line in screen coordinates given origin and angle of the line. We don't care where along the line that point
        /// is, only that it lies on the line and is reasonable distance from center so that we minimize possible rounding errors
        /// </summary>
        /// <param name="heading">Line heading</param>
        /// <param name="radius">Bounding circle radius</param>
        /// <param name="oneOverTanTheta">Out. One over tangent of heading pre-calculated for performance</param>
        /// <returns>X, Y and heading of a bounding line</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private Pose2D GetBoundingLineCharacteristics(double heading, double radius, out double oneOverTanTheta)
        {
            Pose2D linePose = new Pose2D(0, 0, heading); 
            oneOverTanTheta = 1 / Math.Tan(linePose.Heading); // 1 over for perf, to make sure we use multiplication, not division in a loop       

            return new Pose2D(radius * Math.Cos(linePose.Heading), radius * Math.Sin(linePose.Heading), linePose.Heading);
        }

        /// <summary>
        /// This degenerate dot product will let us know if a point is to the left, right or on the line described by start and end points
        /// Assumes center is at 0, 0
        /// </summary>
        /// <param name="controlPointX">Control point x</param>
        /// <param name="controlPointY">Control point y</param>
        /// <param name="pointToExamineX">Test point x</param>
        /// <param name="pointToExamineY">Test point y</param>
        /// <returns>Less than zero if point is to the left, 0 if its on the line, more than 0 if its on the right side of the line</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private double OrientPoint(double controlPointX, double controlPointY, int pointToExamineX, int pointToExamineY)
        {
            return controlPointX * pointToExamineY - controlPointY * pointToExamineX;
        }
    }
}
